/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "fc_sensors.h"
#include "filter.h"
#include "fc_error.h"

extern float F1_wave[12];

#define OPTICAL_FLOW_SAMPLES 5

float optFlowBuffer_x[OPTICAL_FLOW_SAMPLES];
float optFlowBuffer_y[OPTICAL_FLOW_SAMPLES];
struct quickMedianFilter_t
{
    float *buffer;
    uint8_t size;
    uint8_t index;
} opt_mid_filter[2];
float qiuckMedianFilter5(struct quickMedianFilter_t *filter, float data)
{

    filter->buffer[filter->index] = data;
    filter->index++;
    if (filter->index >= filter->size)
    {
        filter->index = 0;
    }
    return quickMedianFilter5f(filter->buffer);
}
void quickMedianFilter5Init(struct quickMedianFilter_t *filter, uint8_t size, float *buffer)
{
    filter->buffer = buffer;
    filter->size = size;
    filter->index = 0;
}
void sensor_opticlFlow_init(void)
{
}
#define RESOLUTION (0.2131946f) /*1m高度下 1个像素对应的位移，单位cm*/
#define OULIER_LIMIT (100)      /*光流像素输出限幅*/
#define VEL_LIMIT (80.f)        /*光流速度限幅*/
static biquadFilter_t optFilter_bf[2];
static biquadFilter_t gyroFilter_bf[2];
bool opt_init = false;

void opticalFlow_apply_layout(int8_t layout, struct rt_sensor_data *opt, fc_sensor_data_t *sensor)
{
    /**
     * @brief after rotation shull be:
     *         ^X
     *         |
     *         |
     *Y<-------|
     */
    switch (layout)
    {
    /**光流摄像头朝向地面*/
    case 0: // X向前
        sensor->optf_raw.optVelX = opt->data.of.y;
        sensor->optf_raw.optVelY = -opt->data.of.x;
        break;
    case 1: // X向后
        sensor->optf_raw.optVelX = -opt->data.of.y;
        sensor->optf_raw.optVelY = opt->data.of.x;
        break;
    case 2: // Y向前
        sensor->optf_raw.optVelX = -opt->data.of.x;
        sensor->optf_raw.optVelY = -opt->data.of.y;
        break;
    case 3: // Y向后
        sensor->optf_raw.optVelX = opt->data.of.x;
        sensor->optf_raw.optVelY = -opt->data.of.y;
        break;
    case 4: // Y向后
        sensor->optf_raw.optVelX = -opt->data.of.y;
        sensor->optf_raw.optVelY = -opt->data.of.x;
        break;
    case 5: // Y向后
        sensor->optf_raw.optVelX = opt->data.of.y;
        sensor->optf_raw.optVelY = opt->data.of.x;
        break;
    default:
        break;
    }
}

void opticalFlowDataUpdate(fc_sensor_data_t *sensor, zdrone_state_t *state)
{
    /**
     * @brief
     *             ^X
     *              |
     *              |
     * Y<———————————|
     *
     */
    float midf_x = qiuckMedianFilter5(&opt_mid_filter[X], sensor->optf_raw.optVelX);
    float midf_y = qiuckMedianFilter5(&opt_mid_filter[Y], sensor->optf_raw.optVelY);
    sensor->optf_raw.lpfX = biquadFilterApply(&optFilter_bf[X], midf_x);
    sensor->optf_raw.lpfY = biquadFilterApply(&optFilter_bf[Y], midf_y);

    float gxLpf = biquadFilterApply(&gyroFilter_bf[X], -state->attitudeRate.pitch * 0.1f);
    float gyLpf = biquadFilterApply(&gyroFilter_bf[Y], state->attitudeRate.roll * 0.1f);

    float fixX = sensor->optf_raw.lpfX - gxLpf;
    float fixY = sensor->optf_raw.lpfY - gyLpf;

    sensor->optf_raw.fixVelX = fixX;
    sensor->optf_raw.fixVelY = fixY;

    // F1_wave[0] = sensor->optf_raw.optVelX;
    // F1_wave[1] = sensor->optf_raw.optVelY;
    // F1_wave[2] = midf_x;
    // F1_wave[3] = midf_y;
    // F1_wave[4] = sensor->optf_raw.lpfX;
    // F1_wave[5] = sensor->optf_raw.lpfY;
    // F1_wave[6] = gxLpf;
    // F1_wave[7] = gyLpf;
    // F1_wave[8] = fixX;
    // F1_wave[9] = fixY;
    // sensor->optf_raw.fixPosX = gxLpf;
    // sensor->optf_raw.fixPosY = gyLpf;



    // sensor->optf_raw.fixVelX = sensor->tof.range / 10.0f * 0.01f * 7.754f * sensor->optf_raw.fixVelX;
    // sensor->optf_raw.fixVelY = sensor->tof.range / 10.0f * 0.01f * 7.754f * sensor->optf_raw.fixVelY;

    // sensor->optf_raw.fixPosX += sensor->optf_raw.fixVelX * 0.01f;
    // sensor->optf_raw.fixPosY += sensor->optf_raw.fixVelY * 0.01f;


    // rt_kprintf("raw:%3d, %3d, %3d \t", sensor->optf_raw.optVelX,sensor->optf_raw.optVelY, sensor->optf_raw.optQual);
    // rt_kprintf("lpf:%3d, %3d, %3d \n", (int)sensor->optf_raw.lpfX,(int)sensor->optf_raw.lpfX, (int)sensor->optf_raw.optQual);
}

void sensor_opticlFlow_handle(struct rt_sensor_data *sensor)
{
    if (sensor->type == RT_SENSOR_CLASS_OP)
    {
        // static uint8_t invalidDataCnt = 0;
        if (!opt_init)
        {
            opt_init = true;
            biquadFilterInitLPF(&optFilter_bf[X], 40, 10000); // NOTE LPF截止频率只能是刷新率的一半
            biquadFilterInitLPF(&optFilter_bf[Y], 40, 10000); // NOTE LPF截止频率只能是刷新率的一半

            biquadFilterInitLPF(&gyroFilter_bf[X], 40, 10000);
            biquadFilterInitLPF(&gyroFilter_bf[Y], 40, 10000);

            quickMedianFilter5Init(&opt_mid_filter[0], OPTICAL_FLOW_SAMPLES, optFlowBuffer_x);
            quickMedianFilter5Init(&opt_mid_filter[1], OPTICAL_FLOW_SAMPLES, optFlowBuffer_y);
        }
        // rt_kprintf("optical flow data: %4d, %4d, %4d, %4d\n", sensor->data.of.x, sensor->data.of.y, sensor->data.of.quality, sensor->data.of.shutter);
        opticalFlow_apply_layout(5, sensor, &fc_sensor_data);
        opticalFlowDataUpdate(&fc_sensor_data, &zdrone_state);

        fc_sensor_data.optf_raw.optQual = sensor->data.of.quality;
        fc_sensor_data.optf_raw.shutter = sensor->data.of.shutter;
        fc_sensor_data.optf_raw.is_valid = sensor->data.of.is_valid;
        fc_sensor_data.optf_raw.isOpFlowOk = (fc_sensor_data.optf_raw.is_valid==1 && fc_sensor_data.optf_raw.optQual > 10 &&
            fc_sensor_data.optf_raw.optQual < 1000);


    }
}
